#ifndef HB_ASSIST_LAND_H
#define HB_ASSIST_LAND_H

#include <iostream>
#include <memory>
#include "apm_flight_handler.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "pid.h"
#include "sift_api.h"


enum siftMarkerType{
	siftMarkerNone = 0,
	siftLandMarker = 1,
	camerScanModeSearch = 2
};

enum goToLandPositionStates{
	notInit = 0,
	setFailed = 1,
	setSuccess = 2
};

class apmFlightHandler;

class AssistLand
{
public:
	AssistLand();
	~AssistLand();

	void init(apmFlightHandler *handler);

	void writeLogFile(const char * format, ...);

	void assistLandRun();

	void assistLand();

	double getFlightCurrentHeigh();

	void resetFlag();

	bool startControlFlight();

	bool goToAlternateLandingPoint();

	//assist land thread
	void controlFlightLand();

	//sift thread
	void computePositionError();

	bool gimbalSetModeAndReset();

	double getFlightCurrentBearing();

	double getGimbalAnglesYaw();

	bool siftHandlerMarker(std::vector<cv::KeyPoint> &markerExKp, cv::Mat &markerExDes,
				std::vector<cv::KeyPoint> &markerInKp, cv::Mat &markerInDes, enum siftMarkerType type);

	void resetTranslationSum();

	void setTranslationSum(Vector3f translationAdd);

	void getTranslationSum(Vector3f &translation);

	void setPositionError(Vector3f positionErr);

	void getPositionError(Vector3f &positionErr, bool &updateFlag);

	//confirm the gimbal camera head is down
	bool gimbalRotate();

	bool cameraScanModeRotateAbsoluteAngle(double rotateRelativeAngle);

	bool cameraScanModeRotateSpeed(double rotateRelativeAngle);

	void computeCorrectBearing(const std::vector<float> data, double kMeasRadius, double &returnAngle);

	void computeMoveBearing(cv::Point imageCenter, std::vector<cv::Point> rect, int inOrEx,
									double &positionErr, double &rotationAngle, enum siftMarkerType type);

	void getFlightVelocity(Vector3f &velocity, int64_t &timestamp);

	
	void controlVelocityXY(PID &txPid, PID &tyPid, Vector3f error, Vector3f &velocity);
	
	float controlVelocityZ();

	double controlYaw(PID &yawPid, double &angleError);

	uint8_t getBatteryInfoPercentage();

	bool velocityControlToCopter(Vector3f velocity, double yawRate);

	MAV_LANDED_STATE getFlightStatus();

	void guidedModeHandler();

private:
	apmFlightHandler *m_apmFlightHandler;
	bool m_assistLandModeRunFlag;

	//flight status
	uint32_t m_flightCurrentMode;
	bool m_armStatus;
	MAV_STATE m_flightSystemStatus;

	//assist land handler
	bool m_goToAlternateLandPoint;
	bool m_assistLandRunFunctionFlag;
	bool m_siftRunFunctionFlag;
	enum goToLandPositionStates m_setLandPosStatus;
	int64_t m_targetPositionLastDistatceUpdateTimeStamp;

	pthread_t m_landRunTid;
	pthread_t m_siftRunTid;

	//handler the camera scan mode
	bool m_ekfInitSuccess;
	bool m_startCameraScanMode;
	enum siftMarkerType m_siftHanderStatus;
	bool m_needAlignCameraYaw;

	Vector3f m_translationSum;
	std::mutex m_translationSumLock;

	Vector3f m_positionError;
	bool m_positionErrorUpdateFlag;
	std::mutex m_positionErrorLock;

	bool m_homeBearingLockFlag;
	double m_computeLandBearing;


};

extern AssistLand assistLand;
#endif //HB_ASSIST_LAND_H
